//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <regs.h>
#include <core.h>
#include <_hal.h>

#include "instruction.h"
#include "exception.h"

static int EmulateLWL(InterruptContext *pContext,
    union unionMipsInstruction instruction)
{
    uint32_t    *pAddr;
    uint32_t    uMemValue;
    uint32_t    uDestValue;
    uint8_t     u8LittleThreeBits;

    //
    // get the real address, erhaps the address is not word aligned;
    // Then we have supposed the CPU is little_Endianness
    // and status regiester's RE bit = 0 .
    //
    pAddr = (uint32_t *)(pContext->regs[instruction.i_format.rs]
                        + instruction.i_format.simmediate);
    u8LittleThreeBits = (uint8_t)((uint32_t)pAddr & 0x3);
    uDestValue = pContext->regs[instruction.i_format.rt];

    pAddr = (uint32_t *)((uint32_t)pAddr & 0xfffffffc);

    //
    // instruction emulation according to the offset value in
    // oprands.
    //
    switch (u8LittleThreeBits) {
        case 0:
            uMemValue = *pAddr;
            uMemValue <<= 24;
            uDestValue &= 0xffffff;
            pContext->regs[instruction.i_format.rt] = uDestValue | uMemValue;
            break;

        case 1:
            uMemValue = *pAddr;
            uMemValue <<= 16;
            uDestValue &= 0xffff;
            pContext->regs[instruction.i_format.rt] = uDestValue | uMemValue;
            break;

        case 2:
            uMemValue = *pAddr;
            uMemValue <<= 8;
            uDestValue &= 0xff;
            pContext->regs[instruction.i_format.rt] = uDestValue | uMemValue;
            break;

        case 3:
            uMemValue = *pAddr;
            pContext->regs[instruction.i_format.rt] = uMemValue;
            break;
    }

    return 0;
}

static int EmulateLWR(InterruptContext *pContext,
    union unionMipsInstruction instruction)
{
    uint32_t    *pAddr;
    uint32_t    uMemValue;
    uint32_t    uDestValue;
    uint8_t     u8LittleThreeBits;

    pAddr = (uint32_t *)(pContext->regs[instruction.i_format.rs]
                        + instruction.i_format.simmediate);
    u8LittleThreeBits = (uint8_t)((uint32_t)pAddr & 0x3);
    uDestValue = pContext->regs[instruction.i_format.rt];

    pAddr = (uint32_t *)((uint32_t)pAddr & 0xfffffffc);

    switch (u8LittleThreeBits) {
        case 0:
            uMemValue = *pAddr;
            pContext->regs[instruction.i_format.rt] = uMemValue;
            break;

        case 1:
            uMemValue = *pAddr;
            uMemValue >>= 8;
            uDestValue &= 0xff000000;
            pContext->regs[instruction.i_format.rt] = uDestValue | uMemValue;
            break;

        case 2:
            uMemValue = *pAddr;
            uMemValue >>= 16;
            uDestValue &= 0xffff0000;
            pContext->regs[instruction.i_format.rt] = uDestValue | uMemValue;
            break;

        case 3:
            uMemValue = *pAddr;
            uMemValue >>= 24;
            uDestValue &= 0xffffff00;
            pContext->regs[instruction.i_format.rt] = uDestValue | uMemValue;
            break;
    }

    return 0;
}

static int EmulateSWL(InterruptContext *pContext,
    union unionMipsInstruction instruction)
{
    uint32_t    *pAddr;
    uint32_t    uMemValue;
    uint32_t    uSourValue;
    uint8_t     u8LittleThreeBits;

    pAddr = (uint32_t *)(pContext->regs[instruction.i_format.rs]
                        + instruction.i_format.simmediate);
    u8LittleThreeBits = (uint8_t)((uint32_t)pAddr & 0x3);
    uSourValue = pContext->regs[instruction.i_format.rt];

    pAddr = (uint32_t *)((uint32_t)pAddr & 0xfffffffc);

    switch (u8LittleThreeBits) {
        case 0:
            uSourValue >>= 24;
            uMemValue = *pAddr;
            uMemValue &= 0xffffff00;
            uMemValue |= uSourValue;
            *pAddr = uMemValue;
            break;

        case 1:
            uSourValue >>= 16;
            uMemValue = *pAddr;
            uMemValue &= 0xffff0000;
            uMemValue |= uSourValue;
            *pAddr = uMemValue;
            break;

        case 2:
            uSourValue >>= 8;
            uMemValue = *pAddr;
            uMemValue &= 0xff000000;
            uMemValue |= uSourValue;
            *pAddr = uMemValue;
            break;

        case 3:
            *pAddr = uSourValue;
            break;
    }

    return 0;
}

static int EmulateSWR(InterruptContext *pContext,
    union unionMipsInstruction instruction)
{
    uint32_t    *pAddr;
    uint32_t    uMemValue;
    uint32_t    uSourValue;
    uint8_t     u8LittleThreeBits;

    pAddr = (uint32_t *)(pContext->regs[instruction.i_format.rs]
                        + instruction.i_format.simmediate);
    u8LittleThreeBits = (uint8_t)((uint32_t)pAddr & 0x3);
    uSourValue = pContext->regs[instruction.i_format.rt];

    pAddr = (uint32_t *)((uint32_t)pAddr & 0xfffffffc);

    switch (u8LittleThreeBits) {
        case 0:
            *pAddr = uSourValue;
            break;

        case 1:
            uSourValue <<= 8;
            uMemValue = *pAddr;
            uMemValue &= 0xff;
            uMemValue |= uSourValue;
            *pAddr = uMemValue;
            break;

        case 2:
            uSourValue <<= 16;
            uMemValue = *pAddr;
            uMemValue &= 0xffff;
            uMemValue |= uSourValue;
            *pAddr = uMemValue;
            break;

        case 3:
            uSourValue <<= 24;
            uMemValue = *pAddr;
            uMemValue &= 0xffffff;
            uMemValue |= uSourValue;
            *pAddr = uMemValue;
            break;
    }

    return 0;
}

int DoRI(InterruptContext *pContext)
{
    union unionMipsInstruction instruction;

    DebugPrint(0, kprintf("trap into DoRI\n"));
    DebugPrint(0, DumpCxt(pContext));

    AcknowledgeException();

    //
    // read the instruction raising exceptions;
    //
    instruction.word = *(uint32_t *)(pContext->cp0_epc
                        + ((pContext->cp0_cause & CAUSEF_BD) ? 4 : 0));

    //
    // branch according to the Op-Code type.
    //
    switch (instruction.i_format.opcode) {
        case lwl_op:
            EmulateLWL(pContext, instruction);
            break;

        case lwr_op:
            EmulateLWR(pContext, instruction);
            break;

        case swl_op:
            EmulateSWL(pContext, instruction);
            break;

        case swr_op:
            EmulateSWR(pContext, instruction);
            break;

        default:
            kprintf("the instruction doese not support\n");
            goto ERROR;
    }

    ComputeReturnEpc(pContext);

    return 0;

ERROR:
    Cli();
    kprintf("DoRI error\n");
    DumpCxt(pContext);
    myChat(pContext);
    return 0;
}
